/*
 Uses an Arduino motorshield
 PWM control for PWM-speed.
 Motor-speed is based on a PID-controller (input from Encoder and output to PWM-speed)
 Motor-speed is set/read with COM-interface
 PID-constants are set with COM-interface
 
 Digital PIN 0, 1
  - COM RX/TX

 Digital PIN 2
  - Interrupt 0 is used for Encoder-B
 
 Digital PIN 3
  - Interrupt 1 is used for Encoder-A

 Digital PIN 4, 5, 6, 7
  - MotorShield PWM control (Motor A and Motor B)
  
 Digital PIN 8
  - Encoder-B direction
 
 Digital PIN 9
  - Encoder-A direction
   
 Digital PIN 10, 11
 Ultrasonic DistanceSensor (Echo Pin 10, Trigger Pin 11)

 Digital PIN 12
  - Servo-0 PWM

*/
#include <PID_v1.h>
#include <Servo.h>
#include <Ultrasonic.h>
#include <digitalWriteFast.h>
#include "MotorControllerCmd.h"

#define ENCODER_B_INTERRUPT_PIN (0) // Encoder-0 Interrupt is on DIGITAL PIN 2
#define ENCODER_A_INTERRUPT_PIN (1) // Encoder-1 Interrupt is on DIGITAL PIN 3
#define ENCODER_B_INTERRUPT_CHANGE_PIN (2) // Encoder-0 is on DIGITAL PIN 2
#define ENCODER_A_INTERRUPT_CHANGE_PIN (3) // Encoder-1 is on DIGITAL PIN 3

#define MOTOR_A_DIRECTION_PIN (4) //MotorShield Motor B
#define MOTOR_A_PWM_PIN (5) //MotorShield Motor B
#define MOTOR_B_DIRECTION_PIN (7) //MotorShield Motor A
#define MOTOR_B_PWM_PIN (6) //MotorShield Motor A
#define ENCODER_B_DIRECTION_PIN (9) // Encoder-0 Direction is on DIGITAL PIN 9
#define ENCODER_A_DIRECTION_PIN (8) // Encoder-1 Direction is on DIGITAL PIN 8
#define DISTANCE_SENSOR_ECHO   (10) // Distance sensor Echo
#define DISTANCE_SENSOR_TRIG   (11) // Distance sensor Trigger
#define SERVO_0_PIN            (12) // Signal for Servo-0 on DIGITAL PIN 12

#define MOTOR_FORWARD (0)
#define MOTOR_REVERSE (1)

volatile int Encoder0TickSpeed = 0;
volatile int Encoder1TickSpeed = 0;
int LastEncoder0TickSpeed = 0;
int LastEncoder1TickSpeed = 0;

long Encoder0TotalTicks = 0;
long Encoder1TotalTicks = 0;

int MotorATargetTickSpeed = 0; //Target ticks pr sample time
int MotorBTargetTickSpeed = 0; //Target ticks pr sample time
int MotorASpeed = 0; //PWM value in range [-255 ; +255]
int MotorBSpeed = 0; //PWM value in range [-255 ; +255]

float MotorA_Kp = 7;
float MotorA_Ki = 5;
float MotorA_Kd = 0;

float MotorB_Kp = 7;
float MotorB_Ki = 5;
float MotorB_Kd = 0;

#define PID_DIVISION_FACTOR (1000.0)
byte MoveByDirection = 0;
float Dir_Kp = 1;
float Dir_Ki = 0;
float Dir_Kd = 0;

float DirSpeed_Kp = 3;
float DirSpeed_Ki = 5;
float DirSpeed_Kd = 0;

int CurrentDirection;
int CorrectionDir;
int MotorATargetDirSpeed = 0;
int MotorBTargetDirSpeed = 0;
int TargetDirSpeed;
int TargetDir;
int CurrentDir;
int CorrectionDirSpeed;
int SumCurrentDir;
int DirFactorCount;
int DirFactor = 1;

PID motorAPID(&LastEncoder0TickSpeed, &MotorASpeed, &MotorATargetTickSpeed, MotorA_Kp, MotorA_Ki, MotorA_Kd, DIRECT);
PID motorBPID(&LastEncoder1TickSpeed, &MotorBSpeed, &MotorBTargetTickSpeed, MotorB_Kp, MotorB_Ki, MotorB_Kd, REVERSE);
PID DirPID(&CurrentDir, &CorrectionDir, &TargetDir, DirSpeed_Kp, DirSpeed_Ki, DirSpeed_Kd, REVERSE);

unsigned long CalculationTime = 0;
unsigned long SampleTime = 40;
int SampleTimeToSmall = false;

Servo servo0; // Define Servo-0
#define SERVO_0_STARTPOSITION (70)

Ultrasonic distanceSensor(DISTANCE_SENSOR_TRIG,DISTANCE_SENSOR_ECHO);

int PositionX;
int PositionY;
float PositionTheta;

int PositionXSaved;
int PositionYSaved;
float PositionThetaSaved;

int MoveDistance = 0;
int TargetDistance = 0;

//float WheelDistance = 167.3;
float DivWheelDistance= 1.0/167.3;
float DivDoubleWheelDistance= 1.0/(2*167.3);

//PositionCorrection
int VehicleTickWidth;
int PositionSampleTime;
int Theta;
float ISumPositionError;
float Div4VehicleTickWidth;
int PositionSumSW;
int PositionCalibration;


void setup() 
{     
  //Attach the interrupt to the input pin and monitor
  attachInterrupt(ENCODER_B_INTERRUPT_PIN, Encoder0Count, CHANGE);
  attachInterrupt(ENCODER_A_INTERRUPT_PIN, Encoder1Count, CHANGE);

  //Set Direction for Encoder pins
  pinMode(ENCODER_B_DIRECTION_PIN, INPUT); 
  pinMode(ENCODER_A_DIRECTION_PIN, INPUT);

  //Set Direction for Motor pins
  pinMode(MOTOR_A_DIRECTION_PIN, OUTPUT);   
  pinMode(MOTOR_B_DIRECTION_PIN, OUTPUT); 
    
  //Set MotorPID
  motorAPID.SetOutputLimits(-255, 255);
  motorBPID.SetOutputLimits(-255, 255);
  DirPID.SetOutputLimits(-15, 15); //TODO Update OutputLimits when TargetDirSpeed is known
  motorAPID.SetMode(AUTOMATIC);
  motorBPID.SetMode(AUTOMATIC);
  DirPID.SetMode(AUTOMATIC);
  
  UpdateSampleTime(SampleTime);
  
  InitialisePositionCorrection();
  
  //Servo setup
  servo0.attach(SERVO_0_PIN);
  servo0.write(SERVO_0_STARTPOSITION);
    
  //Serial Setup
  Serial.begin(9600);           // set up Serial library at 9600 bps  
} 

void Encoder0Count()
{
  if(digitalReadFast(ENCODER_B_DIRECTION_PIN) == digitalReadFast(ENCODER_B_INTERRUPT_CHANGE_PIN))
    Encoder0TickSpeed++;
  else
    Encoder0TickSpeed--;
}

void Encoder1Count()
{
  if(digitalReadFast(ENCODER_A_DIRECTION_PIN) == digitalReadFast(ENCODER_A_INTERRUPT_CHANGE_PIN))
    Encoder1TickSpeed--;
  else
    Encoder1TickSpeed++;
}

void SetMotorSpeed(int DirectionPin, int PWMPin, float Speed)
{
 //Set PWM-Speed and direction for Motor 
 if(Speed > 0)
 {
    digitalWrite(DirectionPin, MOTOR_FORWARD);
    analogWrite(PWMPin, Speed);
 }
 else 
 {
    digitalWrite(DirectionPin, MOTOR_REVERSE);
    analogWrite(PWMPin, -Speed);
 }
}

void HandleCom()
{
  int ReceivedCmd;
  int ReceivedData;
  
  //Byte[0] = cmd
  //Byte[1] = data0
  //Byte[2] = data1  
  if(Serial.available() >= 3) //Cmd contains 3 bytes
  {
    //Receive Cmd and data
    ReceivedCmd = Serial.read();
    ReceivedData = Serial.read(); //MSB
    ReceivedData = ReceivedData << 8;
    ReceivedData |= Serial.read(); //LSB
    
    //Execute Cmd
    long response = ExecuteCmd(ReceivedCmd, ReceivedData);
    char responseLength = ResponseLength[ReceivedCmd];
    if(responseLength > 0)
    {    
      //Send response
      switch(responseLength)
      {
        case 4:
          Serial.write(response >> 24); //MSB
        case 3:
          Serial.write(response >> 16);
        case 2:
          Serial.write(response >> 8);      
        case 1:      
          Serial.write(response & 0xFF); //LSB
      }
    }
  }
}

void UpdateSampleTime(int sampleTime)
{
  SampleTime = sampleTime;
  SampleTimeToSmall = false;
  motorAPID.SetSampleTime(SampleTime);
  motorBPID.SetSampleTime(SampleTime); 
  DirPID.SetSampleTime(SampleTime);  
}

long ExecuteCmd(int cmd, int data)
{
  switch(cmd)
  {
    case CMD_SET_MOTOR_A_TICKSSPEED:
      MotorATargetTickSpeed = data;
      return 0;

    case CMD_SET_MOTOR_B_TICKSSPEED:
      MotorBTargetTickSpeed = data;
      return 0;

    case CMD_SET_DIR:
      TargetDir = data;
      SetTargetMotorSpeed();      
      return 0;
      
    case CMD_SET_DIR_TICKSPEED:
      TargetDirSpeed = data;
      SetTargetMotorSpeed();      
      DirPID.SetOutputLimits(-10, 10);
      return 0;
    
    case CMD_GET_MOTOR_A_DISTANCE:
      return Encoder0TotalTicks;
    
    case CMD_GET_MOTOR_B_DISTANCE:
      return Encoder1TotalTicks;
    
    case CMD_START_POSITION:
      SavePosition();
      PositionX = 0;
      PositionY = 0;
      PositionTheta = 0;
      return 0;

    case CMD_SET_MOVE_DISTANCE:
      Encoder0TotalTicks = 0;
      Encoder1TotalTicks = 0;
      TargetDistance = data;
      MoveDistance = 1;
      return 0;

    case CMD_GET_POSITION_X:
      return PositionXSaved;

    case CMD_GET_POSITION_Y:
      return PositionYSaved;

    case CMD_GET_POSITION_THETA:
      return PositionThetaSaved * PID_DIVISION_FACTOR;

    case CMD_GET_POSITION_Y_ERROR:
      return ISumPositionError * PID_DIVISION_FACTOR;

    case CMD_SET_MOTOR_A_KP:
      MotorA_Kp = data / PID_DIVISION_FACTOR;
      motorAPID.SetTunings(MotorA_Kp, MotorA_Ki, MotorA_Kd);
      return 0;

    case CMD_SET_MOTOR_A_KI:
      MotorA_Ki = data / PID_DIVISION_FACTOR;
      motorAPID.SetTunings(MotorA_Kp, MotorA_Ki, MotorA_Kd);
      return 0;

    case CMD_SET_MOTOR_A_KD:
      MotorA_Kd = data / PID_DIVISION_FACTOR;
      motorAPID.SetTunings(MotorA_Kp, MotorA_Ki, MotorA_Kd);
      return 0;
      
    case CMD_SET_DIRSPEED_KP:
      Dir_Kp = data / PID_DIVISION_FACTOR;
      DirPID.SetTunings(Dir_Kp, Dir_Ki, Dir_Kd);
      return 0;

    case CMD_SET_DIRSPEED_KI:
      Dir_Ki = data / PID_DIVISION_FACTOR;
      DirPID.SetTunings(Dir_Kp, Dir_Ki, Dir_Kd);
      return 0;

    case CMD_SET_DIRSPEED_KD:
      Dir_Kd = data / PID_DIVISION_FACTOR;
      DirPID.SetTunings(Dir_Kp, Dir_Ki, Dir_Kd);
      return 0;

    case CMD_SET_PID_SAMPLETIME:
     UpdateSampleTime(data);
      return 0;

    case CMD_SET_MOTOR_B_KP:
      MotorB_Kp = data / PID_DIVISION_FACTOR;
      motorBPID.SetTunings(MotorB_Kp, MotorB_Ki, MotorB_Kd);
      return 0;

    case CMD_SET_MOTOR_B_KI:
      MotorB_Ki = data / PID_DIVISION_FACTOR;
      motorBPID.SetTunings(MotorB_Kp, MotorB_Ki, MotorB_Kd);
      return 0;

    case CMD_SET_MOTOR_B_KD:
      MotorB_Kd = data / PID_DIVISION_FACTOR;
      motorBPID.SetTunings(MotorB_Kp, MotorB_Ki, MotorB_Kd);
      return 0;

    case CMD_MOVE_STRAIGHT:
      MoveByDirection = data;
      InitialisePositionCorrection();
      PositionSumSW = 0;      
      SumCurrentDir = 0;
      DirFactorCount = 0;      
      return 0;
    
    case CMD_SET_DIR_FACTOR:
      DirFactor = data;
      return 0;
    
    case CMD_SET_DIR_KP:
      SetDirKp(data / PID_DIVISION_FACTOR);
      return 0;

    case CMD_SET_DIR_KI:
      SetDirKi(data / PID_DIVISION_FACTOR);
      return 0;

    case CMD_SET_DIR_KD:
      SetDirKd(data / PID_DIVISION_FACTOR);
      return 0;
            
    case CMD_SET_SERVO_0_POSITION:
      servo0.write(data);
      return 0; 
            
    case CMD_GET_DISTANCESENSOR_MEASURE:
      return distanceSensor.Ranging(CM);

    case CMD_GET_MOTOR_A_TICKSPEED:
      return LastEncoder0TickSpeed;

    case CMD_GET_MOTOR_B_TICKSPEED:
      return LastEncoder1TickSpeed;
      
    case CMD_GET_SAMPLETIME_TO_SMALL:
      return SampleTimeToSmall;
      
    case CMD_GET_LAST_CYCLE_TIME:
      return CalculationTime;      
      
    case CMD_SET_DIR_CALIBRATION:
      PositionCalibration = data;
  }
  return 0;  
}

void InitialisePositionCorrection()
{
  //Initilaized
  VehicleTickWidth = 168; //Width of the vehicle
  PositionSampleTime = 1; //sample time
    
  Theta = 0;
  ISumPositionError = 0;
  Div4VehicleTickWidth = 1.0/((float)4.0*VehicleTickWidth);
}

void SetTargetMotorSpeed()
{
  MotorATargetDirSpeed = TargetDirSpeed + TargetDir/DirFactor;
  MotorBTargetDirSpeed = TargetDirSpeed - TargetDir/DirFactor;  
}

void SetDirKp(float kp)
{
  Dir_Kp = kp/(2*PositionSampleTime*VehicleTickWidth); //DirkP = kP/(2*b*t)
}

void SetDirKi(float ki)
{
  Dir_Ki = ki;
}

void SetDirKd(float kd)
{
  Dir_Kd = kd/(2*VehicleTickWidth*PositionSampleTime*PositionSampleTime); //DirKi = ki/(2*b*t*t)
}

/*
int PositionCorrection()
{  
  //loop
  int sw = LastEncoder0TickSpeed+LastEncoder1TickSpeed;
  int dw = LastEncoder0TickSpeed-LastEncoder1TickSpeed;
  
  PositionSumSW += sw;
  while(PositionSumSW > PositionCalibration)
  {
    sw--;
    dw++;
    PositionSumSW =- PositionCalibration;
  }
        
  Theta = Theta + dw;
  int msdw = sw*dw;
  int swbTheta = sw*Theta*VehicleTickWidth;
  int temp = msdw+swbTheta;
  ISumPositionError = ISumPositionError + (temp+swbTheta)*Div4VehicleTickWidth;
  
  float correction = temp*Dir_Kp; //P-Error
  correction += ISumPositionError*Dir_Ki; //I-Error
  correction += msdw*Dir_Kd; //D-Error
  
  return (int)correction;
}
*/

void PositionCorrection()
{
   SumCurrentDir += (LastEncoder0TickSpeed-LastEncoder1TickSpeed);
   DirFactorCount++;
   if(DirFactorCount == DirFactor)
   {
    CurrentDir = SumCurrentDir;    
     DirPID.Compute();
     DirFactorCount = 1;
     SumCurrentDir = 0;
   }
}

bool PositionChanged(int tickSpeed0, int tickSpeed1)
{
  return(tickSpeed0 > 15 || tickSpeed0 < -15 || tickSpeed1 > 15 || tickSpeed1 < -15);
}

void UpdatePosition()
{
  int sw = LastEncoder0TickSpeed+LastEncoder1TickSpeed;
  int dw = LastEncoder0TickSpeed-LastEncoder1TickSpeed;
  int dCenter = sw;

  PositionTheta = PositionTheta + dw*DivDoubleWheelDistance;
  float cosTheta = 1.0-((PositionTheta*PositionTheta) * 0.5); //1-(Theta^2)/2
  PositionX = PositionX + dCenter * cosTheta;
  PositionY = PositionY + dCenter * PositionTheta;
}

void SavePosition()
{
  PositionThetaSaved = PositionTheta;
  PositionXSaved = PositionX;
  PositionYSaved = PositionY;  
}

void loop() 
{
   unsigned long cycleStartTime = millis();
   
   LastEncoder0TickSpeed = Encoder0TickSpeed;
   Encoder0TickSpeed = 0;  
   LastEncoder1TickSpeed = Encoder1TickSpeed;
   Encoder1TickSpeed = 0;     
   
   Encoder1TotalTicks += LastEncoder1TickSpeed;
   Encoder0TotalTicks += LastEncoder0TickSpeed;
   
   if(MoveByDirection)
   {
     if(MoveDistance && Encoder0TotalTicks >= TargetDistance)
     {
          TargetDirSpeed = 0;
          MoveByDirection = 0;
          MotorATargetTickSpeed = 0;
          MotorBTargetTickSpeed = 0;
     }
     else
     {

//     CorrectionDir = PositionCorrection();
       PositionCorrection();
     
       MotorATargetTickSpeed = MotorATargetDirSpeed - CorrectionDir;
       MotorBTargetTickSpeed = MotorBTargetDirSpeed + CorrectionDir;
     }
   }

   motorAPID.Compute();  
   motorBPID.Compute();

   SetMotorSpeed(MOTOR_A_DIRECTION_PIN, MOTOR_A_PWM_PIN, MotorASpeed);
   SetMotorSpeed(MOTOR_B_DIRECTION_PIN, MOTOR_B_PWM_PIN, MotorBSpeed);       
  
   unsigned long CalculationEndTime = millis();
   
   if(CalculationEndTime > cycleStartTime + SampleTime)
     SampleTimeToSmall = true;

   CalculationTime = CalculationEndTime - cycleStartTime;
   
   unsigned long CycleEndTime = cycleStartTime + SampleTime;
           
   while(millis() < CycleEndTime) 
   {
     HandleCom();
   }
}
